#ifndef APF_H
#define APF_H


#include <vector>
#include <iostream>
#include <cmath>
#include <random>

namespace NS_APF {

struct Vec2
{
    float x;
    float y;

    Vec2() {}
    Vec2(float x, float y): x(x), y(y){}
    void clear(){this->x=0, this->y=0;}

    Vec2 operator= (const Vec2 & _v) {
        this->x = _v.x;
        this->y = _v.y;
        return *this;
    }

    Vec2 operator+ (const Vec2& _v) {
        return Vec2(this->x + _v.x,
                    this->y + _v.y);
    }

    Vec2 operator- (const Vec2 &_v) {
        return Vec2(this->x - _v.x,
                    this->y - _v.y);
    }

    Vec2 operator/ (const float &_v) {
        return Vec2(this->x / _v,
                    this->y / _v);
    }

    Vec2 operator* (const float &_v) {
        return Vec2(this->x * _v,
                    this->y * _v);
    }

    float mod_2() {
        return pow(this->x, 2) + pow(this->y, 2);
    }

    float mod() {
        return sqrt(pow(this->x, 2) + pow(this->y, 2));
    }

    friend std::ostream& operator<<(std::ostream& out,const Vec2& vec2)
    {
        out << "[" << vec2.x << " " << vec2.y << "] ";
        return out;
    }
};


struct node
{
public:
    Vec2 attractive_force_;
    Vec2 revolution_force_;
    Vec2 spring_force_;

    Vec2 sum_force_;
    Vec2 velocity_;
    Vec2 position_;
    Vec2 velocity_calculate();
    void position_update(Vec2 _pos) {
        position_ = _pos;
    }

    Vec2 attractive_force_calculate(Vec2 _object_position, float _k_a, float _goal_max_distance);
    Vec2 spring_force_calculate(Vec2 _object_position, Vec2 relative_position, float _k_s);
    Vec2 revolution_force_calculate(Vec2 _object_position, Vec2 _goal, float _k_r, float _dilation, float _detect_radius);
};

struct APF_Param{
    std::vector<Vec2> obstacle_position_;
    std::vector<Vec2> init_UAV_pos_;
    float k_a_;
    float k_r_;
    float k_s_;
    float UAV_virtual_mass;
    float goal_max_distance_;
    float detect_radius_;
    float UAV_max_speed_;
    float obstacle_dilation_;
};

class APF
{
public:
    std::vector<node> UAV_List_;
    int fol_num_;
    APF_Param param_;
    Vec2 goal_;
    std::vector<std::vector<int>> adjacent_;
    APF(APF_Param* _param_p, Vec2 _goal, int _fol_num);                          
    std::vector<Vec2> calculate(std::vector<Vec2> _current_position_list);
    Vec2 UAV_velocity_calculate(int _UAV_index, Vec2 _current_position);
    double goal_distance();

    void set_goal(Vec2 _goal){
        goal_ = _goal;
    }

    void add_obstacle(Vec2 _start, Vec2 _end);
private:
    void UAV_combine_foce_calculate(int _UAV_index);


};


} // end of namespace NS_APF


#endif // APF_H
